#include "FASTMotor.h"
#include "mc_api.h"
#define FAST_THREAD_MEM 256
#define FAST_THREAD_PRIO 25 // Must be higher than main thread

bool            FASTMotor::inUse = false;
ClassIdentifier FASTMotor::info  = {.name = "FAST Motor Driver", .id = CLSID_MOT_SM1};

FASTMotor::FASTMotor() : CommandHandler("fast", CLSID_MOT_SM1, 0), Thread("FAST", FAST_THREAD_MEM, FAST_THREAD_PRIO)
{
    inUse = true;
    Start();
}
FASTMotor::~FASTMotor() { inUse = false; }

void FASTMotor::turn(int16_t power) { MC_ProgramTorqueRampMotor1(power / 10, 0); }
void FASTMotor::stopMotor() { MC_StopMotor1(); }
void FASTMotor::startMotor()
{
    MC_AcknowledgeFaultMotor1();
    Delay(100);
    MC_StartMotor1();
    registerCommands();
}
bool FASTMotor::motorReady() { return true; } // Returns true if the driver is active and ready to receive commands

void FASTMotor::registerCommands()
{
    CommandHandler::registerCommands();
    registerCommand("errors", FASTMotor_commands::uarterrors, "FAST error flags", CMDFLAG_GET);
    registerCommand("state", FASTMotor_commands::status, "FAST state", CMDFLAG_GET);
    registerCommand("maxtorque", FASTMotor_commands::torque, "Max torque to send for scaling", CMDFLAG_GET);
    registerCommand("vbus", FASTMotor_commands::voltage, "FAST Vbus", CMDFLAG_GET);
    registerCommand("connected", FASTMotor_commands::connected, "FAST connection state", CMDFLAG_GET);
}

CommandStatus FASTMotor::command(const ParsedCommand &cmd, std::vector<CommandReply> &replies)
{

    switch (static_cast<FASTMotor_commands>(cmd.cmdId))
    {
    case FASTMotor_commands::voltage:
        if (cmd.type == CMDtype::get)
        {
            float vbus = VBS_GetAvBusVoltage_d(&BusVoltageSensor_M1._Super) * (3.3f / 0.0625f) / 65535;
            replies.emplace_back(vbus * 1000);
        }
        else
        {
            return CommandStatus::ERR;
        }
        break;

    case FASTMotor_commands::uarterrors:
        if (cmd.type == CMDtype::get)
        {
            replies.emplace_back((uint32_t)MC_GetCurrentFaultsMotor1());
        }
        else
        {
            return CommandStatus::ERR;
        }
        break;

    case FASTMotor_commands::canid: break;
    case FASTMotor_commands::status:
        if (cmd.type == CMDtype::get)
        {
            replies.emplace_back((uint32_t)MC_GetSTMStateMotor1());
        }
        else
        {
            return CommandStatus::ERR;
        }
        break;
    case FASTMotor_commands::torque: {
        if (cmd.type == CMDtype::get)
        {
            int32_t val = MC_GetIqdMotor1().q;
            replies.emplace_back(val);
        }
        else
        {
            return CommandStatus::ERR;
        }
        break;
    }
    case FASTMotor_commands::connected:
        if (cmd.type == CMDtype::get)
        {
            replies.emplace_back(true ? 1 : 0);
        }
        break;

    default: return CommandStatus::NOT_FOUND;
    }

    return CommandStatus::OK;
}